/*
 * @Author: Troubadour 2276791354@qq.com
 * @Date: 2024-03-08 11:57:06
 * @LastEditors: Troubadour 2276791354@qq.com
 * @LastEditTime: 2024-03-30 10:26:11
 * @Version:
 * @Description:
 */

#include "ultrasonic.h"
#include "tim.h"

#define ULTRASONIC_TIMER_HANDLE (&htim4)
#define ULTRASONIC_MAX_TIMEOUT (15000)

Ultrasonic_Type UltrasonicData[ULTRASONIC_QUANTITY];
uint8_t UltrasonicQuantity = sizeof(UltrasonicData) / sizeof(UltrasonicData[0]);
static uint8_t UltrasonicEchoCnt = 0;



UltrasonicState Ultrasonic_GetUpdataState(uint8_t num)
{
    if ((num >= ULTRASONIC_QUANTITY))
    {
        return UTS_RESULT_ERROR;
    }
    return UltrasonicData[num].Updata;
}

double Ultrasonic_GetUpdataDisCm(uint8_t num)
{
    if ((num >= ULTRASONIC_QUANTITY))
    {
        return 0;
    }

    if (UltrasonicData[num].Updata == UTS_UPDATA)
    {
        UltrasonicData[num].Updata = UTS_RESULT_OK;
        return UltrasonicData[num].Distance_cm;
    }
    else
    {
        return 400;
    }
}

double Ultrasonic_GetDisCm(uint8_t num)
{
    if ((num >= ULTRASONIC_QUANTITY))
    {
        return 0;
    }

    return UltrasonicData[num].Distance_cm;
}

void Ultrasonic_Init(void)
{
    /* Configrate gpio */
    UltrasonicData[0].Echo_GPIOx = ECHO_IN1_GPIO_Port;
    UltrasonicData[0].Echo_GPIO_Pin = ECHO_IN1_Pin;
    UltrasonicData[0].Trig_GPIOx = TRIG_OUT1_GPIO_Port;
    UltrasonicData[0].Trig_GPIO_Pin = TRIG_OUT1_Pin;

    for (uint8_t i = 0; i < ULTRASONIC_QUANTITY; i++)
    {
        HAL_GPIO_WritePin(UltrasonicData[i].Trig_GPIOx, UltrasonicData[i].Trig_GPIO_Pin, GPIO_PIN_RESET);
    }
}

void Ultrasonic_Task(void)
{
    uint8_t i = 0;

    /* Clean counter and stop timer. */
    HAL_TIM_Base_Stop(ULTRASONIC_TIMER_HANDLE);
    __HAL_TIM_SET_COUNTER(ULTRASONIC_TIMER_HANDLE, 0);

    /* Enable trigger. */
    for (i = 0; i < UltrasonicQuantity; i++)
    {
        HAL_GPIO_WritePin(UltrasonicData[i].Trig_GPIOx, UltrasonicData[i].Trig_GPIO_Pin, GPIO_PIN_SET);
        UltrasonicData[i].State = UTS_RESULT_ERROR;
    }

    /* Start timer. */
    HAL_TIM_Base_Start(ULTRASONIC_TIMER_HANDLE);
    while (__HAL_TIM_GET_COUNTER(ULTRASONIC_TIMER_HANDLE) < 15)
    {
        /* code */
    }

    /* Clean counter and stop timer. */
    HAL_TIM_Base_Stop(ULTRASONIC_TIMER_HANDLE);
    __HAL_TIM_SET_COUNTER(ULTRASONIC_TIMER_HANDLE, 0);
    /* Disable trigger */
    for (i = 0; i < UltrasonicQuantity; i++)
    {
        HAL_GPIO_WritePin(UltrasonicData[i].Trig_GPIOx, UltrasonicData[i].Trig_GPIO_Pin, GPIO_PIN_RESET);
        UltrasonicData[i].State = UTS_TRIGGER;
        UltrasonicData[i].EchoHighTime_us = 0;
        UltrasonicData[i].EchoLowTime_us = 0;
    }

    /* Clean timer. Wait for the echo to high, up to 200us */
    UltrasonicEchoCnt = 0;
    HAL_TIM_Base_Start(ULTRASONIC_TIMER_HANDLE);

    while ((__HAL_TIM_GET_COUNTER(ULTRASONIC_TIMER_HANDLE) < ULTRASONIC_MAX_TIMEOUT) && (UltrasonicEchoCnt < UltrasonicQuantity))
    {
        for (i = 0; i < UltrasonicQuantity; i++)
        {
            switch (UltrasonicData[i].State)
            {

            case UTS_TRIGGER:
                if (HAL_GPIO_ReadPin(UltrasonicData[i].Echo_GPIOx, UltrasonicData[i].Echo_GPIO_Pin) == GPIO_PIN_SET)
                {
                    UltrasonicData[i].EchoHighTime_us = __HAL_TIM_GET_COUNTER(ULTRASONIC_TIMER_HANDLE);
                    UltrasonicData[i].State = UTS_ECHO;
                }
                break;

            case UTS_ECHO:
                if (HAL_GPIO_ReadPin(UltrasonicData[i].Echo_GPIOx, UltrasonicData[i].Echo_GPIO_Pin) == GPIO_PIN_RESET)
                {
                    UltrasonicData[i].EchoLowTime_us = __HAL_TIM_GET_COUNTER(ULTRASONIC_TIMER_HANDLE);
                    UltrasonicData[i].State = UTS_RESULT_OK;
                    UltrasonicEchoCnt++;
                }
                break;

            case UTS_RESULT_OK:
            default:
                break;
            }
        }
    }
    HAL_TIM_Base_Stop(ULTRASONIC_TIMER_HANDLE);

    /* Calculate result. */
    uint32_t TempSum = 0;

    for (i = 0; i < ULTRASONIC_QUANTITY; i++)
    {
        /* Data check */
        UltrasonicData[i].EchoHighTime_us = (UltrasonicData[i].EchoHighTime_us > UltrasonicData[i].EchoLowTime_us) ? (ULTRASONIC_MAX_TIMEOUT) : (UltrasonicData[i].EchoHighTime_us);
        /* Calculate time. */
#if ULTRASONIC_FILTER_ENABLE
        UltrasonicData[i].TransmitTimeFilter_us[UltrasonicData[i].FilterCnt++] = UltrasonicData[i].EchoLowTime_us - UltrasonicData[i].EchoHighTime_us;
        UltrasonicData[i].FilterCnt %= ULTRASONIC_FILTER_NUMBER;

        TempSum = 0;
        for (uint8_t j = 0; j < ULTRASONIC_FILTER_NUMBER; j++)
        {
            TempSum += UltrasonicData[i].TransmitTimeFilter_us[j];
        }
        UltrasonicData[i].TransmitTime_us = TempSum / ULTRASONIC_FILTER_NUMBER;
#else
        UltrasonicData[i].TransmitTime_us = UltrasonicData[i].EchoLowTime_us - UltrasonicData[i].EchoHighTime_us;
#endif
        /* calculate the distence. */
        UltrasonicData[i].Distance_m = (UltrasonicData[i].TransmitTime_us / 2.0) / 1000000.0 * 340.0;
        UltrasonicData[i].Distance_cm = UltrasonicData[i].Distance_m * 100;
        UltrasonicData[i].Updata = UTS_UPDATA;
    }
}
